14. Implement a Cost Function in C++
In the previous quizzes, you designed a cost function to choose a lane when trying to reach a goal in highway driving:
Here, \Delta d was the lateral distance between the goal lane and the final chosen lane, and \Delta s was the longitudinal distance from the vehicle to the goal.
In this quiz, we'd like you to implement the cost function in C++, but with one important change. The finite state machine we use for vehicle behavior also includes states for planning a lane change right or left (PLCR or PLCL), and the cost function should incorporate this information. We will provide the following four inputs to the function:
- Intended lane: the intended lane for the given behavior. For PLCR, PLCL, LCR, and LCL, this would be the one lane over from the current lane.
- Final lane: the immediate resulting lane of the given behavior. For LCR and LCL, this would be one lane over.
- The \Delta s distance to the goal.
- The goal lane.
Your task in the implementation will be to modify |\Delta d| in the equation above so that it satisifes:
- |\Delta d| is smaller as both intended lane and final lane are closer to the goal lane.
- The cost function provides different costs for each possible behavior: KL, PLCR/PLCL, LCR/LCL.
- The values produced by the cost function are in the range 0 to 1.
You can implement your solution in
cost.cpp
below.
Start Quiz:
#include <iostream>
#include "cost.h"
using std::cout;
using std::endl;
int main() {
int goal_lane = 0;
// Test cases used for grading - do not change.
double cost;
cout << "Costs for (intended_lane, final_lane, goal_distance):" << endl;
cout << "---------------------------------------------------------" << endl;
cost = goal_distance_cost(goal_lane, 2, 2, 1.0);
cout << "The cost is " << cost << " for " << "(2, 2, 1.0)" << endl;
cost = goal_distance_cost(goal_lane, 2, 2, 10.0);
cout << "The cost is " << cost << " for " << "(2, 2, 10.0)" << endl;
cost = goal_distance_cost(goal_lane, 2, 2, 100.0);
cout << "The cost is " << cost << " for " << "(2, 2, 100.0)" << endl;
cost = goal_distance_cost(goal_lane, 1, 2, 100.0);
cout << "The cost is " << cost << " for " << "(1, 2, 100.0)" << endl;
cost = goal_distance_cost(goal_lane, 1, 1, 100.0);
cout << "The cost is " << cost << " for " << "(1, 1, 100.0)" << endl;
cost = goal_distance_cost(goal_lane, 0, 1, 100.0);
cout << "The cost is " << cost << " for " << "(0, 1, 100.0)" << endl;
cost = goal_distance_cost(goal_lane, 0, 0, 100.0);
cout << "The cost is " << cost << " for " << "(0, 0, 100.0)" << endl;
return 0;
}
#include "cost.h"
#include <cmath>
double goal_distance_cost(int goal_lane, int intended_lane, int final_lane,
double distance_to_goal) {
// The cost increases with both the distance of intended lane from the goal
// and the distance of the final lane from the goal. The cost of being out
// of the goal lane also becomes larger as the vehicle approaches the goal.
/**
* TODO: Replace cost = 0 with an appropriate cost function.
*/
double cost = 0;
return cost;
}
#ifndef COST_H
#define COST_H
double goal_distance_cost(int goal_lane, int intended_lane, int final_lane,
double distance_to_goal);
#endif // COST_H